﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Text;

namespace FanucLib
{
    public class Fanuc
    {
        private IPEndPoint endPoint;
        private TcpClient tcpClient;
        private readonly object sync = new object();

        /// <summary>
        /// 初始化通讯参数
        /// </summary>
        /// <param name="robotAddress">机器人的地址</param>
        /// <param name="port">机器人的通讯端口号，默认60008，无需更改</param>
        public Fanuc(string robotAddress, int port = 60008)
        {
            tcpClient = new TcpClient();
            endPoint = new IPEndPoint(IPAddress.Parse(robotAddress), port);
        }

        /// <summary>
        /// 初始化连接
        /// </summary>
        private void Init()
        {
            var ns = tcpClient.GetStream();
            byte[] data = new MessageImpl(4, new NullMessageData()).MessageFrame;
            byte[] response = new byte[4096];
            ns.Write(data, 0, data.Length);
            ns.Read(response, 0, response.Length);
            data = new MessageImpl(1, new InitMessageData()).MessageFrame;
            ns.Write(data, 0, data.Length);
            ns.Read(response, 0, response.Length);
        }

        /// <summary>
        /// 读取数值型寄存器的值
        /// </summary>
        /// <param name="index">寄存器编号</param>
        /// <returns></returns>
        public float ReadNumReg(int index)
        {
            float value = 0.0f;
            lock (sync)
            {
                tcpClient = new TcpClient();
                tcpClient.Connect(endPoint);
                using (NetworkStream ns = tcpClient.GetStream())
                {
                    Init();
                    byte[] response = new byte[4096];
                    byte[] data = new MessageImpl(8, new CmdMessageData("CLRASG")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(9, new CmdMessageData($"SETASG 1 2 R[{index}] 0")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(6, new ValueMessageData(new byte[] { 0x04, 0x08, 0, 0, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0 })).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    int length = ns.Read(response, 0, response.Length);
                    MessageImpl message = new MessageImpl();
                    message.InitializeUnique(response.Take(length).ToArray());
                    value = BitConverter.ToSingle(message.Data.NetworkBytes, 2);
                }
                tcpClient.Close();
                return value;
            }
        }

        /// <summary>
        /// 写入数值型寄存器
        /// </summary>
        /// <param name="index">寄存器编号</param>
        /// <param name="value">要写入的值</param>
        public void WriteNumReg(int index, float value)
        {
            lock (sync)
            {
                tcpClient = new TcpClient();
                tcpClient.Connect(endPoint);
                using (NetworkStream ns = tcpClient.GetStream())
                {
                    Init();
                    byte[] response = new byte[4096];
                    byte[] data = new MessageImpl(8, new CmdMessageData("CLRASG")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(9, new CmdMessageData($"SETASG 1 2 R[{index}] 0")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(8, new ValueMessageData(new byte[] { 0x07, 0x08, 0, 0, 0x02, 0 }.Concat(BitConverter.GetBytes(value)))).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                }
                tcpClient.Close();
            }
        }

        /// <summary>
        /// 读取指定位置型寄存器的值
        /// </summary>
        /// <param name="index">寄存器编号</param>
        /// <returns></returns>
        public float[] ReadPosReg(int index/* ,out short[] config, out float[] joint, out short uf, out short ut, out short validc, out short validj*/)
        {
            List<float> xyzwpr = new List<float>();
            //config = new short[9];
            //joint = new float[7];
            //uf = 0;
            //ut = 0;
            //validc = 0;
            //validj = 0;
            lock (sync)
            {
                tcpClient = new TcpClient();
                tcpClient.Connect(endPoint);
                using (NetworkStream ns = tcpClient.GetStream())
                {
                    Init();
                    byte[] response = new byte[4096];
                    byte[] data = new MessageImpl(8, new CmdMessageData("CLRASG")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(9, new CmdMessageData($"SETASG 1 50 PR[{index}] 0.0")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(6, new ValueMessageData(new byte[] { 0x04, 0x08, 0, 0, 0x32, 0, 0, 0, 0, 0, 0, 0, 0, 0 })).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    int length = ns.Read(response, 0, response.Length);
                    byte[] frame = response.Take(length).Skip(MessageImpl.MinimumFrameSize).ToArray();
                    for (int i = 0; i < 36; i += 4)
                        xyzwpr.Add(BitConverter.ToSingle(frame, i));
                    return xyzwpr.ToArray();
                }
            }
        }

        /// <summary>
        /// 向位置型寄存器写入位置
        /// </summary>
        /// <param name="index">寄存器编号</param>
        /// <param name="value">坐标值</param>
        public void WritePosReg(int index, params float[] value)
        {
            lock (sync)
            {
                tcpClient = new TcpClient();
                tcpClient.Connect(endPoint);
                using (NetworkStream ns = tcpClient.GetStream())
                {
                    Init();
                    byte[] response = new byte[4096];
                    byte[] data = new MessageImpl(8, new CmdMessageData("CLRASG")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(9, new CmdMessageData($"SETASG 1 50 PR[{index}] 0.0")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    List<byte> bytes = new List<byte>(52);
                    for (int i = 0; bytes.Count < 52; i++)
                    {
                        if (i < value.Length)
                            bytes.AddRange(BitConverter.GetBytes(value[i]));
                        else
                            bytes.Add(0);
                    }
                    data = new MessageImpl(9, new ValueMessageData(new byte[] { 0x07, 0x08, 0, 0, 0x1A, 0 }.Concat(bytes)) { MessageLength = 52 }).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(8, new ValueMessageData(new byte[] { 0x07, 0x08, 0x2d, 0, 0x02, 0, 0x01, 0, 0x01, 0 })).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                }
            }
        }

        /// <summary>
        /// 获取机器人当前坐标
        /// </summary>
        /// <param name="uf">指定用户坐标系</param>
        /// <returns></returns>
        public float[] ReadCurPos(int uf)
        {
            //SETASG 1 50 POS[0] 0.0
            List<float> xyzwpr = new List<float>();
            //config = new short[9];
            //joint = new float[7];
            //uf = 0;
            //ut = 0;
            //validc = 0;
            //validj = 0;
            lock (sync)
            {
                tcpClient = new TcpClient();
                tcpClient.Connect(endPoint);
                using (NetworkStream ns = tcpClient.GetStream())
                {
                    Init();
                    byte[] response = new byte[4096];
                    byte[] data = new MessageImpl(8, new CmdMessageData("CLRASG")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(9, new CmdMessageData($"SETASG 1 50 POS[{uf}] 0.0")).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    ns.Read(response, 0, response.Length);
                    data = new MessageImpl(6, new ValueMessageData(new byte[] { 0x04, 0x08, 0, 0, 0x32, 0, 0, 0, 0, 0, 0, 0, 0, 0 })).MessageFrame;
                    ns.Write(data, 0, data.Length);
                    int length = ns.Read(response, 0, response.Length);
                    byte[] frame = response.Take(length).Skip(MessageImpl.MinimumFrameSize).ToArray();
                    for (int i = 0; i < 36; i += 4)
                        xyzwpr.Add(BitConverter.ToSingle(frame, i));
                    return xyzwpr.ToArray();
                }
            }
        }
    }

    /// <summary>
    /// 请求
    /// </summary>
    internal class MessageImpl
    {
        public const int MinimumFrameSize = 56;

        public MessageImpl()
        {
            Data = new NullMessageData();
        }

        public MessageImpl(ushort functionCode, IMessageData data)
        {
            FunctionCode = functionCode;
            Data = data;
        }

        public UInt16 FunctionCode { get; set; }

        private UInt16 MessageHead
        {
            get
            {
                switch (FunctionCode)
                {
                    case 0: return 0;
                    case 4: return 0;
                    case 1: return 8;
                    default: return 2;
                }
            }
        }

        private UInt16 CommandLength
        {
            get
            {
                return (ushort)((Data != null && FunctionCode == 9) ? Data.MessageLength : 0);
            }
        }

        private UInt16 ExtendedFlag
        {
            get
            {
                switch (FunctionCode)
                {
                    case 0: return 0;
                    case 4: return 0;
                    case 9: return 0x0200;
                    default: return 0x0100;
                }
            }
        }

        private UInt16 FunctionMix
        {
            get
            {
                switch (FunctionCode)
                {
                    case 0: return 0;
                    case 4: return 0;
                    case 9: return 0x8009;
                    default: return (ushort)(0xC000 | FunctionCode);
                }
            }
        }

        private UInt16 H0E10
        {
            get
            {
                if (FunctionCode == 0 || FunctionCode == 4)
                    return 0;
                else
                    return 0x0E10;
            }
        }

        private UInt16 H0101
        {
            get
            {
                if (FunctionCode == 0 || FunctionCode == 4)
                    return 0;
                else
                    return 0x0101;
            }
        }

        public IMessageData Data { get; set; }

        public byte[] MessageFrame
        {
            get
            {
                byte[] frame = ProtocolDataUnit;
                byte[] fill = new byte[] { 0x00, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x04, 0x00 };
                return frame.Concat(fill.Skip(10 - (MinimumFrameSize - frame.Length))).ToArray(); // 46<长度<56的数据进行尾部填充
            }
        }

        public virtual byte[] ProtocolDataUnit
        {
            get
            {
                List<byte> pdu = new List<byte>();
                pdu.AddRange(Utility.Uint16ToBytes(MessageHead));
                pdu.AddRange(Utility.Uint16ToBytes(FunctionCode));
                pdu.AddRange(Utility.Uint16ToBytes(CommandLength));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(ExtendedFlag));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(0));

                pdu.AddRange(Utility.Uint16ToBytes(ExtendedFlag));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(0));

                pdu.AddRange(Utility.Uint16ToBytes(FunctionMix));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(H0E10));
                pdu.AddRange(Utility.Uint16ToBytes(0));
                pdu.AddRange(Utility.Uint16ToBytes(H0101));
                if (ExtendedFlag == 0x0200)
                    pdu.AddRange(new byte[] { 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01 });
                if (Data != null)
                    pdu.AddRange(Data.NetworkBytes);
                return pdu.ToArray();
            }
        }

        public void InitializeUnique(byte[] frame)
        {
            if (frame.Length < MinimumFrameSize)
                throw new ArgumentException();
            if ((frame[40] != 0x01) || (frame[41] != 0x01))
                throw new ArgumentException();
            Data = new ResponseMessageData(frame.Skip(42));
        }

        public override string ToString()
        {
            return BitConverter.ToString(MessageFrame);
        }
    }

    internal interface IMessageData
    {
        ushort[] DataArray { get; }
        byte[] NetworkBytes { get; }

        ushort MessageLength { get; }
    }

    internal class NullMessageData : IMessageData
    {
        private static readonly byte[] NullMsg = new byte[] { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

        public virtual byte[] NetworkBytes
        {
            get { return NullMsg; }
        }

        public ushort MessageLength
        {
            get { return 0; }
        }

        public ushort[] DataArray
        {
            get
            {
                List<ushort> data = new List<ushort>();
                for (int i = 0; i < NetworkBytes.Length; i += 2)
                {
                    data.Add(BitConverter.ToUInt16(NetworkBytes, i));
                }
                return data.ToArray();
            }
        }
    }

    internal class InitMessageData : NullMessageData
    {
        private static readonly byte[] InitMsg = new byte[] { 0x4f, 0x01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

        public override byte[] NetworkBytes { get { return InitMsg; } }
    }

    internal class CmdMessageData : IMessageData
    {
        private static readonly byte[] CmdHead = new byte[] { 0x07, 0x38, 0, 0 };

        private string Command;

        public CmdMessageData(string cmd)
        {
            Command = cmd;
        }

        public byte[] NetworkBytes => CmdHead.Concat(Utility.Uint16ToBytes(MessageLength)).Concat(Encoding.ASCII.GetBytes(Command)).ToArray();

        public ushort MessageLength => (ushort)Command.Length;

        public ushort[] DataArray
        {
            get
            {
                List<ushort> data = new List<ushort>();
                for (int i = 0; i < NetworkBytes.Length; i += 2)
                {
                    data.Add(BitConverter.ToUInt16(NetworkBytes, i));
                }
                return data.ToArray();
            }
        }
    }

    internal class ValueMessageData : IMessageData
    {
        public ValueMessageData(IEnumerable<byte> data)
        {
            NetworkBytes = data.ToArray();
        }

        public ushort[] DataArray
        {
            get
            {
                List<ushort> data = new List<ushort>();
                for (int i = 0; i < NetworkBytes.Length; i += 2)
                {
                    data.Add(BitConverter.ToUInt16(NetworkBytes, i));
                }
                return data.ToArray();
            }
        }

        public byte[] NetworkBytes { get; private set; }

        public ushort MessageLength { get; set; }
    }

    internal class ResponseMessageData : IMessageData
    {
        public ResponseMessageData(IEnumerable<byte> data)
        {
            NetworkBytes = data.ToArray();
        }

        public byte[] NetworkBytes { get; private set; }

        public ushort MessageLength => 0;

        public ushort[] DataArray
        {
            get
            {
                List<ushort> data = new List<ushort>();
                for (int i = 0; i < NetworkBytes.Length; i += 2)
                {
                    data.Add(BitConverter.ToUInt16(NetworkBytes, i));
                }
                return data.ToArray();
            }
        }
    }

    public static class Utility
    {
        public static byte[] Uint16ToBytes(UInt16 value)
        {
            return BitConverter.GetBytes(value);
        }

        public static ushort BytesToUInt16(byte[] data)
        {
            return BitConverter.ToUInt16(data, 0);
        }
    }
}